diff --git a/include/cpu/cpu.hh b/include/cpu/cpu.hh
index ab0e4aa..ddc3117 100644
--- a/include/cpu/cpu.hh
+++ b/include/cpu/cpu.hh
@@ -21,12 +21,12 @@ class Cpu {
 
     static constexpr uint8_t GPR_COUNT = 16;
 
-    static constexpr uint8_t GPR_FIQ_FIRST     = 8;
-    static constexpr uint8_t GPR_SVC_FIRST     = 13;
-    static constexpr uint8_t GPR_ABT_FIRST     = 13;
-    static constexpr uint8_t GPR_IRQ_FIRST     = 13;
-    static constexpr uint8_t GPR_UND_FIRST     = 13;
-    static constexpr uint8_t GPR_SYS_USR_FIRST = 8;
+    static constexpr uint8_t GPR_FIQ_FIRST = 8;
+    static constexpr uint8_t GPR_SVC_FIRST = 13;
+    static constexpr uint8_t GPR_ABT_FIRST = 13;
+    static constexpr uint8_t GPR_IRQ_FIRST = 13;
+    static constexpr uint8_t GPR_UND_FIRST = 13;
+    static constexpr uint8_t GPR_OLD_FIRST = 8;
 
     std::shared_ptr<Bus> bus;
     std::array<uint32_t, GPR_COUNT> gpr; // general purpose registers
@@ -54,7 +54,7 @@ class Cpu {
         std::array<uint32_t, GPR_COUNT - GPR_UND_FIRST - 1> und;
 
         // visible registers before the mode switch
-        std::array<uint32_t, GPR_COUNT - GPR_SYS_USR_FIRST> old;
+        std::array<uint32_t, GPR_COUNT - GPR_OLD_FIRST - 1> old;
     } gpr_banked; // banked general purpose registers
 
     struct {
diff --git a/src/cpu/cpu.cc b/src/cpu/cpu.cc
index b8ee55c..6e58909 100644
--- a/src/cpu/cpu.cc
+++ b/src/cpu/cpu.cc
@@ -1,11 +1,9 @@
 #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>
-#include <type_traits>
 
 namespace matar {
 Cpu::Cpu(const Bus& bus) noexcept
@@ -39,13 +37,16 @@ Cpu::chg_mode(const Mode to) {
  * concatenate views */
 #define STORE_BANKED(mode, MODE)                                               \
     std::copy(gpr.begin() + GPR_##MODE##_FIRST,                                \
-              gpr.begin() + gpr.size() - 1,                                    \
+              gpr.end() - 1,                                                   \
               gpr_banked.mode.begin())
 
     switch (from) {
         case Mode::Fiq:
             STORE_BANKED(fiq, FIQ);
             spsr_banked.fiq = spsr;
+            std::copy(gpr_banked.old.begin(),
+                      gpr_banked.old.end() - 2, // dont copy R13 and R14
+                      gpr.begin() + GPR_OLD_FIRST);
             break;
 
         case Mode::Supervisor:
@@ -70,10 +71,15 @@ Cpu::chg_mode(const Mode to) {
 
         case Mode::User:
         case Mode::System:
-            STORE_BANKED(old, SYS_USR);
+            // we only take care of r13 and r14, because FIQ takes care of the
+            // rest
+            gpr_banked.old[5] = gpr[13];
+            gpr_banked.old[6] = gpr[14];
             break;
     }
 
+#undef STORE_BANKED
+
 #define RESTORE_BANKED(mode, MODE)                                             \
     std::copy(gpr_banked.mode.begin(),                                         \
               gpr_banked.mode.end(),                                           \
@@ -83,6 +89,9 @@ Cpu::chg_mode(const Mode to) {
         case Mode::Fiq:
             RESTORE_BANKED(fiq, FIQ);
             spsr = spsr_banked.fiq;
+            std::copy(gpr.begin() + GPR_FIQ_FIRST,
+                      gpr.end() - 2, // dont copy R13 and R14
+                      gpr_banked.old.begin());
             break;
 
         case Mode::Supervisor:
@@ -107,13 +116,17 @@ Cpu::chg_mode(const Mode to) {
 
         case Mode::User:
         case Mode::System:
-            STORE_BANKED(old, SYS_USR);
+            gpr[13] = gpr_banked.old[5];
+            gpr[14] = gpr_banked.old[6];
             break;
     }
 
 #undef RESTORE_BANKED
 
     cpsr.set_mode(to);
+    glogger.info_bold("Mode changed from {:b} to {:b}",
+                      static_cast<uint32_t>(from),
+                      static_cast<uint32_t>(to));
 }
 
 void
diff --git a/src/util/log.hh b/src/util/log.hh
index 1b5762a..1666cfe 100644
--- a/src/util/log.hh
+++ b/src/util/log.hh
@@ -48,6 +48,15 @@ class Logger {
         }
     }
 
+    template<typename... Args>
+    void info_bold(const std::format_string<Args...>& fmt, Args&&... args) {
+        if (level & static_cast<uint8_t>(LogLevel::Info)) {
+            print(stream, "{}{}[INFO] ", ansi::WHITE, ansi::BOLD);
+            log(fmt, std::forward<Args>(args)...);
+            print(stream, ansi::RESET);
+        }
+    }
+
     template<typename... Args>
     void warn(const std::format_string<Args...>& fmt, Args&&... args) {
         if (level & static_cast<uint8_t>(LogLevel::Warn)) {